#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S4,     IRseeker,       sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     LeftMotor,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     RightMotor,    tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     motorF,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    Flipper,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma debuggerWindows("joystickGame");

#include "JoystickDriver.c"
#include "motors.c"
#include "ir.c"

task main()
{
	int DistanceToIr = 0;
	ResetMotorEncoders();
	waitForStart();

	// Approach IR //
	DriveForwardUntilIR(9);
	DistanceToIr = nMotorEncoder[LeftMotor]; // Save Distance Driven

	// Drop Block //
	TurnLeft(50,THREE_QUARTER_IMPULSE); // approximate
	servo[Flipper] = 50;                      //drop block into basket

	// Return to Starting Position //
	TurnRight(50,THREE_QUARTER_IMPULSE);
	DriveDistanceBackwards(DistanceToIr, HALF_IMPULSE);

	// Go Around Ramp //
	TurnLeft(50, THREE_QUARTER_IMPULSE);
	DriveDistanceForward(4000, HALF_IMPULSE);

	// Drive Onto Ramp //
	TurnRight(50, THREE_QUARTER_IMPULSE);
	DriveDistanceForward(4000, THREE_QUARTER_IMPULSE);
}
